package nx.simple3d.data;



/**
 * Class which represents a matrix.
 * @author simon
 *
 */
public class Matrix {
  private static final int MATRIX_CELLS = 16;
  protected double[] m = new double[MATRIX_CELLS];
  private VectorBase vb;
  
  /**
   * Constructs a new identity matrix
   * @param vb the database with vectors to operate on
   */
  public Matrix(){
    this.vb = VectorBase.getInstance();
    m[0]  = 1;
    m[5]  = 1;
    m[10] = 1;
    m[15] = 1;
  }
  
  /**
   * Adds the given matrix to this one
   * @param add
   */
  public void add(Matrix add){
    for(int i = 0; i < 16; i++)
      m[i] += add.m[i];
  }
  
  /**
   * Multiplies the base matrix with the this one and returns
   * a new Matrix as result
   * @param base the base-matrix
   * @return the product (base * this)
   */
  public Matrix applyOn(Matrix base){
    return applyOn(new Matrix(), base);
  }
  
  /**
   * Multiplies the given base matrix with this and returns
   * the result using the result matrix
   * @param result The matrix to write the result to
   * @param base the base-matrix
   * @return the product (base * this) in the given result object
   */
  public Matrix applyOn(Matrix result, Matrix base){
    
    result.m[0] = m[0]*base.m[0] + m[1]*base.m[4] + m[2]*base.m[8] + m[3]*base.m[12];
    result.m[1] = m[0]*base.m[1] + m[1]*base.m[5] + m[2]*base.m[9] + m[3]*base.m[13];
    result.m[2] = m[0]*base.m[2] + m[1]*base.m[6] + m[2]*base.m[10] + m[3]*base.m[14];
    result.m[3] = m[0]*base.m[3] + m[1]*base.m[7] + m[2]*base.m[11] + m[3]*base.m[15];
    
    result.m[4] = m[4]*base.m[0] + m[5]*base.m[4] + m[6]*base.m[8] + m[7]*base.m[12];
    result.m[5] = m[4]*base.m[1] + m[5]*base.m[5] + m[6]*base.m[9] + m[7]*base.m[13];
    result.m[6] = m[4]*base.m[2] + m[5]*base.m[6] + m[6]*base.m[10] + m[7]*base.m[14];
    result.m[7] = m[4]*base.m[3] + m[5]*base.m[7] + m[6]*base.m[11] + m[7]*base.m[15];
    
    result.m[8] = m[8]*base.m[0] + m[9]*base.m[4] + m[10]*base.m[8] + m[11]*base.m[12];
    result.m[9] = m[8]*base.m[1] + m[9]*base.m[5] + m[10]*base.m[9] + m[11]*base.m[13];
    result.m[10] = m[8]*base.m[2] + m[9]*base.m[6] + m[10]*base.m[10] + m[11]*base.m[14];
    result.m[11] = m[8]*base.m[3] + m[9]*base.m[7] + m[10]*base.m[11] + m[11]*base.m[15];
    
    result.m[12] = m[12]*base.m[0] + m[13]*base.m[4] + m[14]*base.m[8] + m[15]*base.m[12];
    result.m[13] = m[12]*base.m[1] + m[13]*base.m[5] + m[14]*base.m[9] + m[15]*base.m[13];
    result.m[14] = m[12]*base.m[2] + m[13]*base.m[6] + m[14]*base.m[10] + m[15]*base.m[14];
    result.m[15] = m[12]*base.m[3] + m[13]*base.m[7] + m[14]*base.m[11] + m[15]*base.m[15];
    
    return result;
  }
  
  /**
   * Applies this matrix on the vector with the given index and
   * stores the result in the vector with the index toVector
   * @param vector the given vector index
   * @param toVector the vector index for the result
   */
  public void apply(int vector, int toVector){
    double x = vb.x[vector];
    double y = vb.y[vector];
    double z = vb.z[vector];
    
    vb.x[toVector] = x*m[0] + y*m[1] + z*m[2] + m[3];
    vb.y[toVector] = x*m[4] + y*m[5] + z*m[6] + m[7];
    vb.z[toVector] = x*m[8] + y*m[9] + z*m[10] + m[11];
  }
  
  /**
   * Applies this matrix on a range of vectors starting at the the given
   * from-index and processing count number of vectors.
   * The results are stored starting at the given to-index.
   * @param from The first input vector index
   * @param to The first output vector index
   * @param count The number of vectors to process
   */
  public void apply(int from, int to, int count){
    double x, y, z;
    int offset = to - from;
    
    for(int vector = from; vector < from + count; vector++){
      x = vb.x[vector];
      y = vb.y[vector];
      z = vb.z[vector];
      
      vb.x[vector + offset] = x*m[0] + y*m[1] + z*m[2] + m[3];
      vb.y[vector + offset] = x*m[4] + y*m[5] + z*m[6] + m[7];
      vb.z[vector + offset] = x*m[8] + y*m[9] + z*m[10] + m[11];
    }
  }
  
  /**
   * Apply this matrix directly to the given vector double-array
   * @param vector A double array with 3 cells
   */
  public void apply(double[] vector){
    double x = vector[0];
    double y = vector[1];
    double z = vector[2];
    
    vector[0] = x*m[0] + y*m[1] + z*m[2] + m[3];
    vector[1] = x*m[4] + y*m[5] + z*m[6] + m[7];
    vector[2] = x*m[8] + y*m[9] + z*m[10] + m[11];
  }
  
  /**
   * Translates this matrix in the direction of the given vector
   * @param vector The index of the vector which holds the translation
   */
  public void translate(int vector){
    m[3]  = m[0] * vb.x[vector] + m[1] * vb.y[vector] + m[2]  * vb.z[vector] + m[3];
    m[7]  = m[4] * vb.x[vector] + m[5] * vb.y[vector] + m[6]  * vb.z[vector] + m[7];
    m[11] = m[8] * vb.x[vector] + m[9] * vb.y[vector] + m[10] * vb.z[vector] + m[11];
  }
  
  /**
   * Translates this matrix in the direction of the given vector
   * @param vector A double array vector with 3 cells
   */
  public void translate(double[] vector){
    m[3]  = m[0] * vector[0] + m[1] * vector[1] + m[2]  * vector[2] + m[3];
    m[7]  = m[4] * vector[0] + m[5] * vector[1] + m[6]  * vector[2] + m[7];
    m[11] = m[8] * vector[0] + m[9] * vector[1] + m[10] * vector[2] + m[11];
  }
  
  /**
   * Rotates this matrix by the given angle around the X-Axis
   * @param angle The angle in radians
   */
  public void rotateX(double angle){
    double sin = Math.sin(angle);
    double cos = Math.cos(angle);
    double a;
    a = m[1]*cos + m[2]*sin;
    m[2] = m[1]*-sin + m[2]*cos;
    m[1] = a;
    a = m[5]*cos + m[6]*sin;
    m[6] = m[5]*-sin + m[6]*cos;
    m[5] = a;
    a = m[9]*cos + m[10]*sin;
    m[10] = m[9]*-sin + m[10]*cos;
    m[9] = a;
    a = m[13]*cos + m[14]*sin;
    m[14] = m[13]*-sin + m[14]*cos;
    m[13] = a;
  }
  
  /**
   * Rotates this matrix by the given angle around the Y-Axis
   * @param angle The angle in radians
   */
  public void rotateY(double angle){
    double sin = Math.sin(angle);
    double cos = Math.cos(angle);
    double a;    
    a = m[0]*cos + m[2]*-sin;
    m[2] = m[0]*sin + m[2]*cos;
    m[0] = a;
    a = m[4]*cos + m[6]*-sin;
    m[6] = m[4]*sin + m[6]*cos;
    m[4] = a;
    a = m[8]*cos + m[10]*-sin;
    m[10] = m[8]*sin + m[10]*cos;
    m[8] = a;
    a = m[12]*cos + m[14]*-sin;
    m[14] = m[12]*sin + m[14]*cos;
    m[12] = a;
  }
  
  /**
   * Rotates this matrix by the given angle around the Z-Axis
   * @param angle The angle in radians
   */
  public void rotateZ(double angle){
    double sin = Math.sin(angle);
    double cos = Math.cos(angle);
    double a;
    a = m[0]*cos + m[1]*sin;
    m[1] = m[0]*-sin + m[1]*cos;
    m[0] = a;
    a = m[4]*cos + m[5]*sin;
    m[5] = m[4]*-sin + m[5]*cos;
    m[4] = a;
    a = m[8]*cos + m[9]*sin;
    m[9] = m[8]*-sin + m[9]*cos;
    m[8] = a;
    a = m[12]*cos + m[13]*sin;
    m[13] = m[12]*-sin + m[13]*cos;
    m[12] = a;
  }
  
  /**
   * Inverts this matrix
   */
  public void invert(){
    m = getInverted().m;
  }

  /**
   * Returns the inverse of this matrix without modifying this
   * @return the inverse of this matrix
   */
  public Matrix getInverted(){
    Matrix result = new Matrix();
    updateInverted(result);
    return result;
  }
  
  
  public void updateInverted(Matrix r){

    double s0 = m[0] * m[5] - m[4] * m[1];
    double s1 = m[0] * m[6] - m[4] * m[2];
    double s2 = m[0] * m[7] - m[4] * m[3];
    double s3 = m[1] * m[6] - m[5] * m[2];
    double s4 = m[1] * m[7] - m[5] * m[3];
    double s5 = m[2] * m[7] - m[6] * m[3];
                            
    double c5 = m[10]* m[15] - m[14] * m[11];
    double c4 = m[9] * m[15] - m[13] * m[11];
    double c3 = m[9] * m[14] - m[13] * m[10];
    double c2 = m[8] * m[15] - m[12] * m[11];
    double c1 = m[8] * m[14] - m[12] * m[10];
    double c0 = m[8] * m[13] - m[12] * m[9];

    double invdet = 1 / (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);

    r.m[0] = (m[5]  * c5 - m[6]  * c4 + m[7]  * c3) * invdet;
    r.m[1] = (-m[1] * c5 + m[2]  * c4 - m[3]  * c3) * invdet;
    r.m[2] = (m[13] * s5 - m[14] * s4 + m[15] * s3) * invdet;
    r.m[3] = (-m[9] * s5 + m[10] * s4 - m[11] * s3) * invdet;

    r.m[4] = (-m[4] * c5 + m[6]  * c2 - m[7]  * c1) * invdet;
    r.m[5] = (m[0]  * c5 - m[2]  * c2 + m[3]  * c1) * invdet;
    r.m[6] = (-m[12]* s5 + m[14] * s2 - m[15] * s1) * invdet;
    r.m[7] = (m[8]  * s5 - m[10] * s2 + m[11] * s1) * invdet;

    r.m[8] = (m[4]  * c4 - m[5]  * c2 + m[7]  * c0) * invdet;
    r.m[9] = (-m[0] * c4 + m[1]  * c2 - m[3]  * c0) * invdet;
    r.m[10]= (m[12] * s4 - m[13] * s2 + m[15] * s0) * invdet;
    r.m[11]= (-m[8] * s4 + m[9]  * s2 - m[11] * s0) * invdet;

    r.m[12]= (-m[4] * c3 + m[5]  * c1 - m[6]  * c0) * invdet;
    r.m[13]= (m[0]  * c3 - m[1]  * c1 + m[2]  * c0) * invdet;
    r.m[14]= (-m[12]* s3 + m[13] * s1 - m[14] * s0) * invdet;
    r.m[15]= (m[8]  * s3 - m[9]  * s1 + m[10] * s0) * invdet;
  }
  
  /**
   * Prints out this vector in a nicely readable format
   */
  public String toString(){
    return m[0] + " | " + m[1] + " | " + m[2]  + " | " + m[3] + "\n"
         + m[4] + " | " + m[5] + " | " + m[6]  + " | " + m[7] + "\n"
         + m[8] + " | " + m[9] + " | " + m[10] + " | " + m[11] + "\n"
         + m[12] + " | " + m[13] + " | " + m[14] + " | " + m[15];
  }
}
